#include<iostream>
#include<iomanip>
#include<cwiid.h>
#include<bluetooth/bluetooth.h>
#include"sendkey.h"
#include<signal.h>

cwiid_mesg_callback_t cwiid_callback;
using namespace std;
bool end = 0;
void set_led_state(cwiid_wiimote_t *wiimote, unsigned char led_state)
{
	if (cwiid_set_led(wiimote, led_state)) {
		fprintf(stderr, "Error setting LEDs \n");
	}
}
	
void set_rpt_mode(cwiid_wiimote_t *wiimote, unsigned char rpt_mode)
{
	if (cwiid_set_rpt_mode(wiimote, rpt_mode)) {
		fprintf(stderr, "Error setting report mode\n");
	}
}
int main(int argc, char** argv)
{
	cwiid_wiimote_t *wiimote;	/* wiimote handle */
	struct cwiid_state state;	/* wiimote state */
	bdaddr_t bdaddr;	/* bluetooth device address */
	sigset_t sigset;
	int signum;

	sigemptyset(&sigset);
	sigaddset(&sigset, SIGTERM);
	sigaddset(&sigset, SIGINT);
	sigaddset(&sigset, SIGUSR1);
	
	/* Connect to address given on command-line, if present */
	if (argc > 1) {
		str2ba(argv[1], &bdaddr);
	}
	else {
		bdaddr = *BDADDR_ANY;
	}

	init_sendkey();


	/* Connect to the wiimote */
	printf("Put Wiimote in discoverable mode now (press 1+2)...\n");
	if (!(wiimote = cwiid_open(&bdaddr, 0))) {
		fprintf(stderr, "Unable to connect to wiimote\n");
		return -1;
	}
	if (cwiid_set_mesg_callback(wiimote, cwiid_callback)) {
		fprintf(stderr, "Unable to set message callback\n");
	}

	if (cwiid_enable(wiimote, CWIID_FLAG_MESG_IFC)) {
	  fprintf(stderr, "Error enabling messages\n");
	}
	
	set_rpt_mode(wiimote, CWIID_RPT_BTN);

	/* wait */
	sigprocmask(SIG_BLOCK, &sigset, NULL);
	sigwait(&sigset, &signum);
	sigprocmask(SIG_UNBLOCK, &sigset, NULL);


	cout << "Closing sendkey" <<endl;
	close_sendkey();
	if (cwiid_close(wiimote)) {
	  fprintf(stderr, "Error on wiimote disconnect\n");
	  return -1;
	}


  return 0;
}


void cwiid_callback(cwiid_wiimote_t *wiimote, int mesg_count,
                    union cwiid_mesg mesg[], struct timespec *timestamp)
{
  int i;
  for (i=0; i < mesg_count; i++)
    {
      switch (mesg[i].type) {
      case CWIID_MESG_BTN:
	cout << "Buttons: " << setw(4) << (hex) << mesg[i].btn_mesg.buttons << endl;
	if(mesg[i].btn_mesg.buttons & CWIID_BTN_B)
	  sendkey(KEY_SPACE);
	if(mesg[i].btn_mesg.buttons & CWIID_BTN_LEFT)
	  sendkey(KEY_LEFT);
	if(mesg[i].btn_mesg.buttons & CWIID_BTN_RIGHT)
	  sendkey(KEY_RIGHT);
	if(mesg[i].btn_mesg.buttons & CWIID_BTN_UP)
	  sendkey(KEY_UP);
	if(mesg[i].btn_mesg.buttons & CWIID_BTN_DOWN)
	  sendkey(KEY_DOWN);
	if(mesg[i].btn_mesg.buttons & CWIID_BTN_HOME)
	  sendkey(KEY_BACKSPACE);
	if(mesg[i].btn_mesg.buttons & CWIID_BTN_MINUS)
	  sendkey(KEY_VOLUMEDOWN);
	if(mesg[i].btn_mesg.buttons & CWIID_BTN_PLUS)
	  sendkey(KEY_VOLUMEUP);
	if(mesg[i].btn_mesg.buttons & CWIID_BTN_1)
	  sendkey(KEY_PAGEUP);
	if(mesg[i].btn_mesg.buttons & CWIID_BTN_2)
	  sendkey(KEY_PAGEDOWN);

	if(mesg[i].btn_mesg.buttons & CWIID_BTN_A)
	  set_rpt_mode(wiimote, CWIID_RPT_ACC | CWIID_RPT_BTN);
	else
	  set_rpt_mode(wiimote, CWIID_RPT_BTN);

	break;
      case CWIID_MESG_ACC:
	cout << "Acc_X: " << setw(6) << (int) mesg[i].acc_mesg.acc[CWIID_X]
	     << "Acc_Y: " << setw(6) << (int) mesg[i].acc_mesg.acc[CWIID_Y]
	     << "Acc_Z: " << setw(6) << (int) mesg[i].acc_mesg.acc[CWIID_Z] << endl;

	break;
      case CWIID_MESG_ERROR:
	if (cwiid_close(wiimote)) {
	  fprintf(stderr, "Error on wiimote disconnect\n");
	  exit(-1);
	}
	exit(0);
	break;
      default:
	break;
      }
    }


}
